
/**************************************************************************************************************
 **author:xiezh
 **date:2020-6-4
 **udp通信接收端。将接受到的数据通过topic系统内转发
 **端口号6666
 **************************************************************************************************************/
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <errno.h>

#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sstream>

#define IP "192.168.91.130"
#define PORT 7777

int main(int argc, char **argv)
{
	ros::init(argc, argv, "server_topic");
	ros::NodeHandle n,n_para("~");
	ros::Publisher pub = n.advertise<std_msgs::String>("/ctrl/server",1);

	int cnt = -1;
	int sfd = -1;
	socklen_t len = -1;
	struct sockaddr_in ser_addr;
	struct sockaddr_in from_addr;
	char buf[128] = {0};
	char ser_buf[128] = {0};

	//socket
	sfd = socket(AF_INET, SOCK_DGRAM, 0);
	if(0 > sfd)
	{
		perror("socket error:");
		goto _out;
	}
	
	//bind
	{
		//init
		ser_addr.sin_family = AF_INET;
		ser_addr.sin_port = htons(PORT);
		ser_addr.sin_addr.s_addr = INADDR_ANY;//inet_addr(IP);
	}
	cnt = bind(sfd, (struct sockaddr *)&ser_addr, sizeof(struct sockaddr));
	if(0 > cnt)
	{
		perror("bind error:");
		goto _out;
	}

	while(ros::ok())
	{
		//recvfrom
		len = sizeof(struct sockaddr);
		memset(&from_addr, 0, sizeof(struct sockaddr));
		memset(buf, 0, sizeof(buf));
		cnt = recvfrom(sfd, buf, sizeof(buf), 0, (struct sockaddr *)&from_addr, &len);
		if(0 > cnt)
		{
			perror("received error:");
			goto _out;
		}
		printf("buf = %s\n",buf);
		printf("cnt = %d\n",cnt);

		std_msgs::String cmd;
		for (int i=0;i<16;++i)
        	 {
               	 	cmd.data.push_back(buf[i]);
            	}
		pub.publish(cmd);
		if(0 > cnt)
		{
			perror("sendto error:");
			goto _out;
		}
		
		
	}
	
	//close
	close(sfd);
	return 0;
_out:
	return -1;
}
